#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <unistd.h>
#include <sys/time.h> 
#include "XAG_ThreadWrapper.h"


#define RTK_UDP_SERVER_PORT 8899
#define BUFF_LEN 128

XAG_BOOL rtk_udp_server_ThreadCallback(XAG_VOID * handle);
XAG_VOID rtk_udp_server_ThreadFunc();

void handle_udp_msg_hex(int fd)
{
    unsigned char buf[BUFF_LEN];  //接收缓冲区，1024字节
    socklen_t len;
    int count = 0, i = 0;
    struct sockaddr_in clent_addr;  //clent_addr用于记录发送方的地址信息
	struct timeval start, end;
	float  timeuse;

    printf("------------- handle_udp_msg_hex 2-----------\n");

	system("camera_if_direct 0x0 0xb 0x2");

    while(1)
    {
        memset(buf, 0, BUFF_LEN);
        len = sizeof(clent_addr);

		system("camera_if_direct 0x0 0xb 0x3");

		gettimeofday( &start, NULL );
        count = recvfrom(fd, buf, BUFF_LEN, 0, (struct sockaddr*)&clent_addr, &len);  //recvfrom是拥塞函数，没有数据就一直拥塞
        if(count == -1)
        {
            printf("recieve data fail!\n");
            return;
        }
		gettimeofday( &end, NULL );
		timeuse = 1000000 * ( end.tv_sec - start.tv_sec ) + end.tv_usec - start.tv_usec; 
		timeuse /= 1000;

		printf("------------ timeuse = %f\n ------------", timeuse);

           printf("------------- %s -----------\n", "begin");
        for (i = 0; i < count; i++) {
            printf("%.2x ",buf[i]);
        }
           printf("------------- %s -----------\n", "end");
		
		usleep(50000);
    }
}

/*
    server:
            socket-->bind-->recvfrom-->sendto-->close
*/

int rtk_upd_server_init()
{
    int server_fd, ret;
    struct sockaddr_in ser_addr;

    server_fd = socket(AF_INET, SOCK_DGRAM, 0); //AF_INET:IPV4;SOCK_DGRAM:UDP
    if(server_fd < 0)
    {
        printf("create socket fail!\n");
        return -1;
    }

    memset(&ser_addr, 0, sizeof(ser_addr));
    ser_addr.sin_family = AF_INET;
    ser_addr.sin_addr.s_addr = htonl(INADDR_ANY); //IP地址，需要进行网络序转换，INADDR_ANY：本地地址
    ser_addr.sin_port = htons(RTK_UDP_SERVER_PORT);  //端口号，需要网络序转换

    ret = bind(server_fd, (struct sockaddr*)&ser_addr, sizeof(ser_addr));
    if(ret < 0)
    {
        printf("socket bind fail!\n");
        return -1;
    }

    //handle_udp_msg(server_fd);   //处理接收到的数据
	handle_udp_msg_hex(server_fd);   //处理接收到的数据

    close(server_fd);
    return 0;
}

XAG_VOID rtk_udp_init()
{
    XAG_CHAR *pCodePosition;
    pthread_t  tPositioningPid; 
    
    XAG_CODE_POSITION_GET(pCodePosition);
    XAG_ThreadWrapper::XAG_ThreadWrapper_Creat(&tPositioningPid, rtk_udp_server_ThreadCallback, (void *)0, pCodePosition);
    
    printf("============= rtk_udp_init =============\n");
}

XAG_BOOL rtk_udp_server_ThreadCallback(XAG_VOID * handle)
{
    rtk_udp_server_ThreadFunc();
}

XAG_VOID rtk_udp_server_ThreadFunc()
{
	rtk_upd_server_init();
}

